In [11]:
#def calcSlipParams3D2(IC, m, FS, ymin, T, P0 = [14000., 1.16, 1., 0., 0.]
# why do these params do not work?? -> fixed?
import models.fitSlip as fs
import models.slip as sl
# orig
IC = array([1.200, 3.995, 0. ])
ymin = 1.109
FS = array([1.212, 4.081, 0.])
T = 0.330
# testing
#IC = array([1.200, 2.995, 0. ])
#ymin = 1.119
#FS = array([1.222, 3.081, 0.])
#T = 0.3585
#FS = array([1.14, 3.82, 0.])
#ymin = 1.04129
#T = 0.330
#T = 0.24
mass = 106.
m = mass #/ mass
P0 = [25000. / mass, 1.2, 1.15, 0., 0.]
P0 = [ 197.95746255 , 1.29969924 , 1.22344742 , 0. , 0.378988 ]
P0 = [ 197.95746255 , 1.2496851 , 1.23703141 , 0. , 0.464988 ]
#[ 197.95746255 1.25037771 1.23672615 0. 0.464988 ]
P0 = [ (148.87275418 + 35) * m, 1.2496851 , 1.24676386 , 0. , 0.464988 * m ]
par_active = [0,1,2,3] # all 4 dims
goals_active = [0,1,2,3]
par_active = [0,1,2,3] # omit k (keep constant)
goals_active = [0,1,2,3] # deactivate one goal function (T =0, ymin=1, yfinal =2, vzfinal = 3)
#par_active = [0,1]
#goals_active = [0,2]
debug_out = False # display debug info
print "p0:", P0
#P0 = [3.70843920e+04 , 1.29723028e+00 , 1.20341403e+00, 0.00000000e+00,
# 4.92887280e+01]
rcond0 = 1e-6
if True:
"""
calculates a set of SLIP parameters that result in the desired motion.
:args:
IC (3x float) : initial condition y, vx, vz
FS (3x float) : final state y, vx, vz
ymin : minimal vertical excursion
T : total step duration
P0 (5x float) : initial guess for parameters [k, alpha, L0, beta, dE]
(dE is ignored. It is calculated from IC and FS)
:returns:
[k, alpha, L0, beta, dE] parameter vector that results in the desired state
"""
dE = (FS[0]-IC[0])*m*9.81 + .5*m*(FS[1]**2 + FS[2]**2
- IC[1]**2 - IC[2]**2)
k, alpha, L0, beta, _ = P0
def getDiff(t, y):
""" returns the difference in desired params """
delta = [T - t[-1],
ymin - min(y[:,1]),
#FS - y[-1,[1,3,5]],
FS[[0,2]] - y[-1,[1,5]]
]
return hstack(delta)
rcond = rcond0 # for pinverting the jacobian. this will be adapted during the process
init_success = False
while not init_success:
try:
pars = [k, L0, m, alpha, beta, dE]
t, y = sl.qSLIP_step3D(IC, pars)
init_success = True
except ValueError:
L0 -= .02
d0 = getDiff(t, y)[par_active]
nd0 = norm(d0)
print "initial delta:", norm(d0)
#print \"difference: \", nd0, d0
cancel = False
niter = 0
while nd0 > 1e-6 and not cancel:
niter += 1
if niter > 20:
print "too many iterations"
cancel = True
break
# calculate jacobian dDelta / dP
#print \"rep:\", niter
hs = array([10, .001, .001, .001])
pdims = [0, 1, 3, 4]
J = []
for dim in range(len(pdims)):
parsp = [k, L0, m, alpha, beta, dE]
parsm = [k, L0, m, alpha, beta, dE]
parsp[pdims[dim]] += hs[dim]
parsm[pdims[dim]] -= hs[dim]
t, y = sl.qSLIP_step3D(IC, parsp)
dp = getDiff(t, y)
t, y = sl.qSLIP_step3D(IC, parsm)
dm = getDiff(t, y)
J.append((dp - dm)/(2*hs[dim]))
J = vstack(J).T
if debug_out:
print "orig J-shape:", J.shape
print "det J:", det(J)
# take only "active" (variable) parameters and goal states
J = J[:, par_active][goals_active, :]
if debug_out:
print "new J-shape:", J.shape
print "det J:", det(J)
pred = array([k, L0, alpha, beta])[par_active]
update = dot(pinv(J, rcond=rcond), d0)
#nrm = norm(update * array([.001, 10, 10, 10]))
success = False
cancel = False
rep = 0
while not (success or cancel):
rep += 1
#print \"update:\", update
if rep > 10:
# reset
if rcond < 1e-12:
print "rcond too small!"
cancel = True
break
else:
rcond /= 100
pars = [k, L0, m, alpha, beta, dE]
if debug_out:
print "recomputing with better resolution"
break
update_stretch = zeros(4)
update_stretch[par_active] = update
dk, dL0, dalpha, dbeta = update_stretch
pars = [k - dk, L0 - dL0, m, alpha - dalpha, beta - dbeta, dE]
try:
t, y = sl.qSLIP_step3D(IC, pars)
d0 = getDiff(t, y)[par_active]
#print "d0 = ", d0
except (ValueError, sl.SimFailError, TypeError):
if debug_out:
print "escaping ..."
update /= 2
continue
if norm(d0) < nd0:
success = True
nd0 = norm(d0)
if debug_out:
print "update found!" + (" {:3.3f} " * 3).format(*(array(pars)[array([0,1,3])]))
else:
update /= 2
#print \"difference: \", norm(d0), d0
k, L0, alpha, beta = array(pars)[[0,1,3,4]]
print "final delta:", nd0
#if not cancel:
# print \"converged!\"
pars_out = array([k, alpha, L0, beta, dE])
if debug_out:
print "final pars:", pars_out
print "--------- SIM WITH NEW PARS --------------"
p0l = pars_out # [k, alpha, L0, beta, dE]
p_sim = [p0l[0] , p0l[2], m, p0l[1], p0l[3], p0l[4]]
t, res = sl.qSLIP_step3D(IC, p_sim )
print "sim:", res[-1,[1,3,5]]
print "ref:",FS
print "time: sim:", t[-1], "ref:", T
In [12]:
p_sim
Out[12]:
In [17]:
# run the simulation
t, y = sl.qSLIP_step3D(IC, p_sim)
tf = arange(0,t[-1],0.01)
fyi = gradient(interp(tf, t, y[:,4] )) * 100 * m
yi = interp(tf, t, y[:,1] )
figure(figsize=(16,8))
plot(t,y[:,1],'b', label='CoM height')
ylabel('CoM height [m]')
xlabel('time [s]')
legend(loc='upper left' )
twinx()
plot(tf, fyi, 'r', label='force')
ylabel('vertical force [N]')
legend()
Out[17]:
In [4]:
# "kinetic" stiffness estimate (assuming that nadir == VLO)
L0 = p_sim[1]
y_min = min(y[:,1]l)
dk, dl = sl.dk_dL(L0, p_sim[0], y_min, p_sim[5])
print "initial k:", p_sim[0]
print "new k:", p_sim[0] + dk
#print "new L:", L0 + dl
print "kinetic estimate (pre):", max(fyi) / (L0 - y_min)
print "kinetic estimate (post):", max(fyi) / (L0 + dl - y_min)
print "-" * 50
print "differences are due to difference in nadir and VLO events"
print "further, they also indicate potential experimental inaccuracies"
In [5]:
figure()
plot(yi, fyi,'b')
xlabel('CoM height')
ylabel('Fz')
Out[5]:
In [ ]: